#include "additivePID.hpp"
#include "utils.hpp"

AdditivePID::AdditivePID(float P, float I, float D)
{
    this->P = P;
    this->I = I;
    this->D = D;
    Reset();
}
void AdditivePID::SetP(float value)
{
    this->P = value;
}
void AdditivePID::SetI(float value)
{
    this->I = value;
}
void AdditivePID::SetD(float value)
{
    this->D = value;
}

float AdditivePID::GetControl(float error)
{
    ctrl += D * (error - 2 * lastError + lastSecondError);
    ctrl += P * (error - lastError);
    ctrl += I * error;
    lastSecondError = lastError;
    lastError = error;
    return ctrl;
}

float AdditivePID::GetControlTrick1(float error)
{
    bool sign = (error - lastError) * error >= 0;
    if (sign)
    {
        ctrl += D * (error - 2 * lastError + lastSecondError);
        ctrl += P * (error - lastError);
        ctrl += I * error;
    }
    lastSecondError = lastError;
    lastError = error;
    return ctrl;
}

float AdditivePID::GetControlWithClamp(float error, float range)
{
    GetControl(error);
    ctrl = ABSClamp(ctrl, range);
    return ctrl;
}

void AdditivePID::Reset()
{
    lastError = 0;
    lastSecondError = 0;
    ctrl = 0;
}